package ai.application;

import ai.agents.Agent;
import ai.spatial.Vector;

public class Environment {

	private double minX;
	private double minY;
	private double maxX;
	private double maxY;

	public Environment(double minX, double minY, double maxX, double maxY){
		this.minX = minX;
		this.minY = minY;
		this.maxX = maxX;
		this.maxY = maxY;
	}
	
	public void moveAgent(Agent agent) {
		Vector desiredVelocity = new Vector(0.3 * agent.getDesiredVelocity().getX(), 0.3 * agent.getDesiredVelocity().getY());//a.getDesiredVelocity();
		Vector currentVelocity = agent.getCurrentVelocity();
		double accelerationX = Config.getSimStepSize()*(currentVelocity.getX() + desiredVelocity.getX())/Config.TAU;
		double accelerationY = Config.getSimStepSize()*(currentVelocity.getY() + desiredVelocity.getY())/Config.TAU;
		double newVelocityX = currentVelocity.getX() + accelerationX;
		double newVelocityY = currentVelocity.getY() + accelerationY;
		
		// restrict velocity to max value
		double scale = validateVelocity(newVelocityX, newVelocityY);
		newVelocityX = scale * newVelocityX;
		newVelocityY = scale * newVelocityY;
		double moveVelocityX = newVelocityX * Config.getSimStepSize();
		double moveVelocityY = newVelocityY * Config.getSimStepSize();
		
		double newPositionX = agent.getPosition().getX() + moveVelocityX;
		double newPositionY = agent.getPosition().getY() + moveVelocityY;
		Vector newPosition = new Vector(newPositionX, newPositionY);
		
		if (!checkCollision(newPosition)) {
			agent.setPosition(newPosition);
			agent.getCurrentVelocity().setX(newVelocityX);
			agent.getCurrentVelocity().setY(newVelocityY);
		} else {
			agent.getCurrentVelocity().setX(0);
			agent.getCurrentVelocity().setY(0);
		}		
	}
	
	private boolean checkCollision(Vector newPosition) {
		if (newPosition.getX() < this.getMinX()) {
			return true;
		}
		if (newPosition.getY() < this.getMinY()) {
			return true;
		}
		if (newPosition.getX() > this.getMaxX()) {
			return true;
		}
		if (newPosition.getY() > this.getMaxY()) {
			return true;
		}
		return false;
	}

	private double validateVelocity(double newVelocityX, double newVelocityY) {

		double v = Math.hypot(newVelocityX, newVelocityY);
		if (v > Config.MAX_V) {
			return Config.MAX_V / v;
		}
		return 1;
	}
	
	public double getMinX() {
		return minX;
	}
	
	public double getMinY() {
		return minY;
	}
	
	public double getMaxX() {
		return maxX;
	}
	
	public double getMaxY() {
		return maxY;
	}
}
